#include "rviz_custom_plugins/viewmodels/point_group_model.h"
#include <QStringBuilder>
#include <QJsonObject>
#include "rviz_custom_plugins/utils/interactive_marker_util.h"


using rviz_custom_plugins::utils::getMetadataItem;
using rviz_custom_plugins::utils::addMetadata;


namespace rviz_custom_plugins
{


namespace viewmodels
{


PointGroupModel::PointGroupModel(QObject *parent): QStandardItemModel(parent) {
    setColumnCount(4);
    setHorizontalHeaderLabels({ "desc", "x", "y", "z" });

    groupTitlePrefix = "组";
    groupSignPrefix = "point_group_sign_";
}


Qt::ItemFlags PointGroupModel::flags(const QModelIndex& index) const {
    Qt::ItemFlags flags = QStandardItemModel::flags(index);
    return flags & ~Qt::ItemIsEditable;
}


void PointGroupModel::setGroupTitlePrefix(QString prefix)
{
    groupTitlePrefix = prefix;
}


QString PointGroupModel::generateGroupSignName()
{
    QString markerName = groupSignPrefix % QString::fromStdString(std::to_string(++incrGroupSignId));
    return markerName;
}


std::string& PointGroupModel::getGroupSign(int l1Row)
{
    auto it = groupSigns.begin();
    std::advance(it, l1Row);
    return *it;
}


bool PointGroupModel::appendGroup()
{
    // add group sig
    QString signName = generateGroupSignName();
    groupSigns.push_back(signName.toStdString());

    // add marker list
    markersList.push_back(std::list<InteractiveMarker>());

    // add model row
    QString itemName = groupTitlePrefix % QString::number(groupSigns.size());
    QStandardItem* modelItem = new QStandardItem(itemName);
    appendRow(modelItem);
    return true;
}


void PointGroupModel::removeGroup(int l1Row)
{
    // remove marker list
    auto it = markersList.begin();
    std::advance(it, l1Row);
    markersList.erase(it);

    // remove group sign
    auto signIt = groupSigns.begin();
    std::advance(signIt, l1Row);
    groupSigns.erase(signIt);

    // remove model row
    removeRow(l1Row);

    for(int i = l1Row; i < rowCount(); ++i) {
        QStandardItem* groupItem = item(i, 0);
        QString groupName = groupTitlePrefix % QString::number(i+1);
        groupItem->setData(groupName, Qt::DisplayRole);
    }
}


std::list<InteractiveMarker>& PointGroupModel::getGroupItems(int l1Row)
{
    auto it = markersList.begin();
    std::advance(it, l1Row);
    return *it;
}


InteractiveMarker& PointGroupModel::getGroupItem(int l1Row, int l2Row)
{
    auto it = markersList.begin();
    std::advance(it, l1Row);
    std::list<InteractiveMarker>& markers = *it;
    auto markerIt = markers.begin();
    std::advance(markerIt, l2Row);
    return *markerIt;
}


bool PointGroupModel::insertGroupItem(int l1Row, int l2Row, InteractiveMarker& intMarker)
{
    auto & markers = getGroupItems(l1Row);
    auto markerIt = markers.begin();
    std::advance(markerIt, l2Row); // 移动迭代器到第l1Row个位置
    markers.insert(markerIt, intMarker);

    QList<QStandardItem*> itemRow;
    itemRow << new QStandardItem(QString::fromStdString("顶点") % QString::number(l2Row+1));
    itemRow << new QStandardItem(QString::number(intMarker.pose.position.x,'f', 2));
    itemRow << new QStandardItem(QString::number(intMarker.pose.position.y,'f', 2));
    itemRow << new QStandardItem(QString::number(intMarker.pose.position.z,'f', 2));
    QStandardItem* groupRow = item(l1Row, 0);
    groupRow->insertRow(l2Row, itemRow);

    // 重置行号
    for(int i = l2Row; i < groupRow->rowCount(); ++i) {
        QStandardItem* child = groupRow->child(i, 0);
        QString markerDesc = QString::fromStdString("顶点") % QString::number(i+1);
        child->setData(markerDesc, Qt::DisplayRole);
    }

    handleItemsReorder(l1Row, l2Row);

    emit itemOrderUpdated(l1Row, l2Row, markers);
    return true;
}


void PointGroupModel::removeGroupItem(int l1Row, int l2Row) {

    auto & markers = getGroupItems(l1Row);
    auto it = markers.begin();
    std::advance(it, l2Row);
    markers.erase(it);

    QModelIndex groupIndex = index(l1Row, 0);
    // 删除二级节点
    removeRow(l2Row, groupIndex);

    QStandardItem* groupRow = item(l1Row, 0);
    // 重置行号
    for(int i = l2Row; i < groupRow->rowCount(); ++i) {
        QStandardItem* child = groupRow->child(i, 0);
        QString markerDesc = QString::fromStdString("顶点") % QString::number(i+1);
        child->setData(markerDesc, Qt::DisplayRole);
    }

    handleItemsReorder(l1Row, l2Row);

    emit itemOrderUpdated(l1Row, l2Row, markers);
}


void PointGroupModel::handleItemsReorder(int l1Row, int l2Row)
{
    auto & markers = getGroupItems(l1Row);
    auto it = markers.begin();
    std::advance(it, l2Row); // 移动到指定索引

    int tmpL2Row = l2Row;
    for ( ;it != markers.end(); ++it,++tmpL2Row) {
        // 重新设置marker的row
        // QJsonObject metadata;
        // metadata[L1_ROW] = QString::number(l1Row);
        // metadata[L2_ROW] = QString::number(tmpL2Row);
        // setMetadata(*it, metadata);
        addMetadata(*it, L1_ROW, QString::number(l1Row));
        addMetadata(*it, L2_ROW, QString::number(tmpL2Row));
    }
}


void PointGroupModel::handleGroupsReorder(int l1Row)
{
    auto markersIt = markersList.begin();
    std::advance(markersIt, l1Row); // 移动到指定索引

    int tmpL1Row = l1Row;

    for( ; markersIt != markersList.end(); ++markersIt, ++tmpL1Row)
    {
        int tmpL2Row = 0;
        for(auto markerIt = markersIt->begin(); markerIt != markersIt->end(); ++markerIt, ++tmpL2Row)
        {
            // 重新设置marker的row
            // QJsonObject metadata;
            // metadata[L1_ROW] = QString::number(tmpL1Row);
            // metadata[L2_ROW] = QString::number(tmpL2Row);
            // setMetadata(*markerIt, metadata);

            addMetadata(*markerIt, L1_ROW, QString::number(l1Row));
            addMetadata(*markerIt, L2_ROW, QString::number(tmpL2Row));
        }
    }
}


void PointGroupModel::getMarkerRow(QString& markerName, int& l1Row, int& l2Row)
{
    int l1Index = 0;
    for (auto it = markersList.begin(); it != markersList.end(); ++it, ++l1Index) {
        std::list<InteractiveMarker>& markers = *it;
        int l2Index = 0;
        for (auto markerIt = markers.begin(); markerIt != markers.end(); ++markerIt, ++l2Index) {
            if(markerIt->name == markerName.toStdString()) {
                l1Row = l1Index;
                l2Row = l2Index;
                return;
            }
        }
    }
}


void PointGroupModel::handleMarkerFeedback(InteractiveMarkerServer::FeedbackConstSharedPtr feedback)
{
    // 获取到点的位置
    int l1Row = -1;
    int l2Row = -1;
    QString markerName = QString::fromStdString(feedback->marker_name);
    getMarkerRow(markerName, l1Row, l2Row);

    // 修改点的pose
    auto it = markersList.begin();
    std::advance(it, l1Row);
    std::list<InteractiveMarker>& markers = *it;

    auto markerIt = markers.begin();
    std::advance(markerIt, l2Row);
    markerIt->pose = feedback->pose;

    QModelIndex groupIndex = index(l1Row, 0);  // 一级节点索引
    setData(index(l2Row, 1, groupIndex), QString::number(feedback->pose.position.x,'f', 2), Qt::DisplayRole);
    setData(index(l2Row, 2, groupIndex), QString::number(feedback->pose.position.y,'f', 2), Qt::DisplayRole);
    setData(index(l2Row, 3, groupIndex), QString::number(feedback->pose.position.z,'f', 2), Qt::DisplayRole);
}


std::list<std::list<InteractiveMarker>>& PointGroupModel::getGroups()
{
    return markersList;
}




}


}
